                             # coding=utf-8
import socket
import time

import RPi.GPIO as GPIO

from config import Config
from infraredsensor import InfraredSensor
from lcdscreen import Screen
from microswitch import MicroSwith
from photoelectricsensor import PhotoElectricSensor
from irswitch import Irswitch
from stepper import Stepper
from lock import Lock
from stepper import Twosteppers


def Get_local_ip():
    """
    Returns the actual ip of the local machine.
    This code figures out what source address would be used if some traffic
    were to be sent out to some well known address on the Internet. In this
    case, a Google DNS server is used, but the specific address does not
    matter much. No traffic is actually sent.
    """
    try:
        csock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        csock.connect(('123.125.115.110', 80))
        (addr, port) = csock.getsockname()
        csock.close()
        return addr
    except socket.error:
        return "127.0.0.1"

'''
def initMotors():

    # 初始化MA电机
    ma_pulpin = int(Config().read("Stepper", "MA_pul_pin"))
    ma_dirpin = int(Config().read("Stepper", "MA_dir_pin"))   
    ma_reverse = int(Config().read("Stepper", "MA_REVERCE"))
    ma_cycle = int(Config().read("Stepper", "MA_cycle"))
    ma_switch_low = PhotoElectricSensor(int(Config().read("PhotoElectric", "SAL_pin")))
    ma_switch_middle = PhotoElectricSensor(int(Config().read("PhotoElectric", "SAM_pin")))
    ma_switch_high = PhotoElectricSensor(int(Config().read("PhotoElectric", "SAH_pin")))
    ma_stepper = Stepper(ma_pulpin, ma_dirpin, ma_reverse, ma_cycle, ma_switch_low,ma_switch_middle,ma_switch_high )
    ma_stepper.run_motor_positon(0)


    # 初始化MCX电机
    mcx_pulpin = int(Config().read("Stepper", "MCX_pul_pin"))
    mcx_dirpin = int(Config().read("Stepper", "MCX_dir_pin"))
    mcx_reverse = int(Config().read("Stepper", "MCX_REVERCE"))
    mcx_cycle = int(Config().read("Stepper", "MCX_cycle"))
    mcx_switch = PhotoElectricSensor(int(Config().read("PhotoElectric", "SCX_pin")))
    mcx_stepper = Stepper(mcx_pulpin, mcx_dirpin, mcx_reverse, mcx_cycle, mcx_switch)
    mcx_stepper.motor_to_origin()
    

    # 初始化MCY电机
    mcy_pulpin = int(Config().read("Stepper", "MCY_pul_pin"))
    mcy_dirpin = int(Config().read("Stepper", "MCY_dir_pin"))
    mcy_reverse = int(Config().read("Stepper", "MCY_REVERCE"))
    mcy_cycle = int(Config().read("Stepper", "MCY_cycle"))
    mcy_switch = PhotoElectricSensor(int(Config().read("PhotoElectric", "SCY_pin")))
    mcy_stepper = Stepper(mcy_pulpin, mcy_dirpin, mcy_reverse, mcy_cycle, mcy_switch)
    mcy_stepper.motor_to_origin()


    #初始化MBL电机
    mbl_pulpin = int(Config().read("Stepper", "MBL_pul_pin"))
    mbl_dirpin = int(Config().read("Stepper", "MBL_dir_pin"))
    mbl_reverse = int(Config().read("Stepper", "MBL_REVERCE"))
    mbl_cycle = int(Config().read("Stepper", "MBL_cycle"))
    mbl_stepper = Stepper(mbl_pulpin, mbl_dirpin, mbl_reverse, mbl_cycle)


    #初始化MBM电机
    mbm_pulpin = int(Config().read("Stepper", "MBM_pul_pin"))
    mbm_dirpin = int(Config().read("Stepper", "MBM_dir_pin"))
    mbm_reverse = int(Config().read("Stepper", "MBM_REVERCE"))
    mbm_cycle = int(Config().read("Stepper", "MBM_cycle"))
    mbm_stepper = Stepper(mbm_pulpin, mbm_dirpin, mbm_reverse, mbm_cycle)
    

    #初始化MBR电机
    mbr_pulpin = int(Config().read("Stepper", "MBR_pul_pin"))
    mbr_dirpin = int(Config().read("Stepper", "MBR_dir_pin"))
    mbr_reverse = int(Config().read("Stepper", "MBR_REVERCE"))
    mbr_cycle = int(Config().read("Stepper", "MBR_cycle"))
    mbr_stepper = Stepper(mbr_pulpin, mbr_dirpin, mbr_reverse, mbr_cycle)
    '''
    

def initLock():
    '''
    初始化锁状态
    :return:

    lock = Lock(Config().read("Sys_Pins", "lock_pin"))
    
    gpio = Config().read("PhotoElectric", "SLK_pin")
    print(gpio)   
    gpio = Config().read("PhotoElectric", "SLK_pin")
    slk = PhotoElectricSensor(gpio)
    '''

    lock = Lock(25)
    gpio = 11 #Config().read("PhotoElectric", "SLK_pin")
    print(gpio)
    photoElectricSensor = PhotoElectricSensor(gpio)
    while photoElectricSensor.checkObstacle() != 1:  # 门未上锁
        screen.displayData("lock the door please")  # 显示数据
        time.sleep(1)
    




def initScreen():
    '''
    初始化屏幕
    :return:
    '''
    screen = Screen(bus=1, addr=0x27, cols=16, rows=2)
    ip = Get_local_ip()
    print(ip)
    screen.displayData(ip)  # 显示数据
    return screen


if __name__ == '__main__':
    try:

        screen = initScreen()
        print("初始化屏幕完成")
        time.sleep(4) 
        screen.displayData("initializing...") # 初始化
        time.sleep(2)


        #initLock()
        lock = Lock(25)
        gpio = 11 #Config().read("PhotoElectric", "SLK_pin")
        print(gpio)
        slk = PhotoElectricSensor(gpio)
        while slk.checkObstacle() != 1:  # 门未上锁
              screen.displayData("Please lock door") 
              time.sleep(1)
    
        print("初始化锁状态完成")
        time.sleep(0.5) 


   
        # 初始化电机
        bmbl_steppers = Twosteppers(17,4,18,15,-1,0.01)
        
        bmbr_steppers = Twosteppers(17,4,22,27,-1,0.01)
       
        ma_switch_low = PhotoElectricSensor(5)
        ma_switch_middle = PhotoElectricSensor(6)
        ma_switch_high = PhotoElectricSensor(13)
        ma_stepper = Stepper(21, 20, 1, 0.05, ma_switch_low, ma_switch_middle, ma_switch_high)
        ma_stepper.run_motor_position(0)
              
        mcx_switch = PhotoElectricSensor(19)
        mcx_stepper = Stepper(7, 8, -1, 0.01, mcx_switch, 0, 0)
        mcx_stepper.motor_to_origin() # mcx归零
        

        mcy_switch = PhotoElectricSensor(26)
        mcy_stepper = Stepper(16, 12, -1, 0.01, mcy_switch,0 ,0)
        mcy_stepper.motor_to_origin() # mcy归零

        #mcy_stepper.run_motor_offset(16360)
        #mcx_stepper.run_motor_offset(24480)

        #mcy_stepper.run_motor_offset(14800)

        #bmbl_steppers.run_motors(1800,-1,0.01) # 包裹移入左侧货架

        print("初始化电机状态完成")
        time.sleep(0.5)  # 延迟500ms
        


        #初始化红外开关
        #sbml = Irswith(Config().read("Sys_Pins", "SBML_pin"))
        #sbmr = Irswith(Config().read("Sys_Pins", "SBMR_pin"))
        sbml=Irswitch(10)
        sbmr=Irswitch(9)

        #》》》》》》》》》》》》》》》》》》》测试代码
        '''
        ma_stepper.run_motor_position(1)

        bmbl_steppers.run_motors(12000,-1,0.001)

        while sbmr.checkObstacle() != 1:
            bmbl_steppers.run_motors(50,1,0.001)

        bmbr_steppers.run_motors(12000,1,0.001)
        
        while sbml.checkObstacle() != 1:
            bmbr_steppers.run_motors(50,-1,0.001)

     
        ma_stepper.run_motor_position(2)
        ma_stepper.run_motor_position(1)

        bmbl_steppers.run_motors(12000,-1,0.001)

        while sbmr.checkObstacle() != 1:
            bmbl_steppers.run_motors(50,1,0.001)

        bmbr_steppers.run_motors(12000,1,0.001)
        while sbml.checkObstacle() != 1:
            bmbr_steppers.run_motors(50,-1,0.001)

        '''
        
        screen.displayData("initialized") # 初始化完成
        time.sleep(2)

        screen.displayData("Pls select mode") 

        while True:
            param = None
            while True:
                print("监听事件")
                # 微动开关01监听
                microSwith01 = MicroSwith(23) # (Config().read("MicroSwith", "MicroSwith_pin01"))
                if microSwith01.checkOnOrOff() == 1:  # 按下按键跳出控制
                    print("监听到按键01")
                    param = "01号开关被触发"
                    break
                time.sleep(0.1)  # 延迟100ms

                # 微动开关02监听
                microSwith02 = MicroSwith(24) # (Config().read("MicroSwith", "MicroSwith_pin02"))
                if microSwith02.checkOnOrOff() == 1:  # 按下按键跳出控制
                    print("监听到按键02")
                    param = "02号开关被触发"
                    break
                time.sleep(0.1)  # 延迟100ms

                # 红外传感器监听
                infraredSensor = InfraredSensor(14) # (Config().read("Sys_Pins", "infraredSensor_pin"))
                if infraredSensor.checkObstacle() == 1:  # 有障碍物警告
                    print("监听到红外传感器")
                    param = "红外传感器监测有障碍物"
                    break
                time.sleep(0.1)  # 延迟100ms


            #模式1
            if param == "01号开关被触发":
                
               screen.displayData("receving Pack...") 
               
               ma_stepper.run_motor_position(0)              
               lock.openLock() # 开门
               time.sleep(5)
               while True:
                    screen.displayData("Please lock door") 
                    if slk.checkObstacle() == 1:
                       screen.displayData("receving Pack...")  
                       break
               mcy_stepper.run_motor_offset(16360) # 夹持件夹紧
               mcx_stepper.run_motor_offset(24480)

               ma_stepper.run_motor_position(1) # 升降至中间位置
               
               
               mcx_stepper.motor_to_origin() # 夹持件放松
               mcy_stepper.run_motor_offset(14800)

               bmbl_steppers.run_motors(10000,-1,0.001) # 包裹移入左侧货架
               
               screen.displayData("Pls select mode") 
               

               
            #模式2   
            elif param == "02号开关被触发":
                
                screen.displayData("fetching Pack...")
                
                ma_stepper.run_motor_position(1) # 升降至中间位置
                
                mcx_stepper.motor_to_origin() # 夹持件归零
                mcy_stepper.motor_to_origin()

                while sbml.checkObstacle() != 1: # 从右侧货架移出
                  bmbr_steppers.run_motors(50,-1,0.001)
                   
                mcy_stepper.run_motor_offset(16360) # 夹持件夹紧
                mcx_stepper.run_motor_offset(24480)
                
                ma_stepper.run_motor_position(0) # 升降至最低位置
                
                mcy_stepper.run_motor_offset(10000) # 夹持件放松
                mcx_stepper.run_motor_offset(10000)

                
                time.sleep(0.1)
                lock.openLock() # 开门
                time.sleep(5)
                while True:
                    screen.displayData("Please lock door") 
                    if slk.checkObstacle() == 1:
                       screen.displayData("fetching Pack...") 
                       break
                    
                
                screen.displayData("Pls select mode")
                

                
             #模式3   
            elif param == "红外传感器监测有障碍物":
                
              screen.displayData("loading Pack...")
              
              #卸载包裹
              mcx_stepper.run_motor_offset(10000) # 夹持件放松
              mcy_stepper.run_motor_offset(10000)
              
              ma_stepper.run_motor_position(2)  # 升降至最高位置
              
              time.sleep(30) # 等待无人机卸载
              
              mcy_stepper.run_motor_offset(16360) # 夹持件夹紧
              mcx_stepper.run_motor_offset(24480)
              
              ma_stepper.run_motor_position(1) # 升降至中间位置
              
              mcx_stepper.motor_to_origin() # 夹持件放松
              mcy_stepper.run_motor_offset(14800)

              bmbr_steppers.run_motors(10000,1,0.001) # 包裹转入右侧货架
              
              
              #装载包裹
              mcy_stepper.run_motor_offset(1000)
              
              while sbmr.checkObstacle() != 1: # 包裹从左侧货架移出
                 bmbl_steppers.run_motors(50,1,0.001)             
                   
              mcy_stepper.run_motor_offset(16360) # 夹持件夹紧
              mcx_stepper.run_motor_offset(24480)
              
              ma_stepper.run_motor_position(2) # 升降至最高位置

              mcy_stepper.run_motor_offset(10000) # 夹持件放松
              mcx_stepper.run_motor_offset(10000)
              
              time.sleep(30) # 等待无人机装载

              ma_stepper.run_motor_position(0) # 升降至最低位置

              screen.displayData("Pls select mode")

              
            else:
                pass

    except KeyboardInterrupt:
        GPIO.cleanup()
